#ifndef __MAIN_H__
#define __MAIN_H__
// ------------------------------------------------------------------------------
//   Includes
// ------------------------------------------------------------------------------

#include <iostream>
#include <stdio.h>
#include <cstdlib>
#include <unistd.h>
#include <cmath>
#include <string.h>
#include <inttypes.h>
#include <fstream>
#include <signal.h>
#include <time.h>
#include <sys/time.h>

using std::string;
using namespace std;

#include <common/mavlink.h>

#include "pilot/autopilot_interface.h"
#include "pilot/serial_port.h"
#include "controller/controller_3D_posPID.h"
#include "controller/controller_angPID.h"
#include "navigator/waypoint_list.h"
#include "navigator/ref_navigator.h"
#include "vision/xy_vision.h"
#include "filter/filter_j1.h"
#include "ultrasonic/ultrasonic.h"
#include "commander/commander.h"
#include "common/integrator_time.h"
#include "log/log.h"
#include "wireless_debug/wireless_debug.h"
#include "HealthManage/HealthManage.h"
#include "counter/times_counter.h"

#define DIS_CRITERIA_HORIZONTAL 	10		//m
#define DIS_CRITERIA_TAKEOFF  		0.5		//m

#define SAMPLING_PERIOD 			0.02  	//s

#define HEIGHT_TO_SET_YAW			3.0		//m, used in takeoff phase
#define HEIGHT_ACCELERATION			3.0		//m

//below used in landing phase
#define HEIGHT_DIFFERENCE			0.0							//m	(land point altitude - takeoff point altitude)
#define HEIGHT_ULTRA_ADAPTABLE		(HEIGHT_DIFFERENCE + 4)		//m
#define HEIGHT_ULTRA_OPEN			(HEIGHT_DIFFERENCE + 10)	//m
#define HEIGHT_HOVER_FIND_MARK		(HEIGHT_DIFFERENCE + 15)	//m
#define HEIGHT_LAND_STAGE_1			(HEIGHT_DIFFERENCE + 10)	//m
#define HEIGHT_LAND_STAGE_2			(HEIGHT_DIFFERENCE + 5)		//m
#define HEIGHT_OPEN_CAMERA			(HEIGHT_DIFFERENCE + 20)	//m
#define HEIGHT_DECELERATION			(HEIGHT_DIFFERENCE + 10)	//m
#define HEIGHT_LANDING				1.5							//m

#define DESCEND_RATE				1.0							// m/s
#define LANDING_RATE				0.4							// m/s

//for remote manual land
#define VISION_WIDTH_LAND
#define VISION_HEIGHT_LAND

#define COE_IMAGE_TO_DIS			0.0018

//yaw angle to launch flight
#define ANGLE_CRITERIA_INTITIAL   	(5.0 / 180.0 * 3.1415926)

// -----------------------------------------------------------------------------
//   PID control parameters
// -----------------------------------------------------------------------------

//takeoff controller
#define KP_X_TAKEOFF 		0.4
#define KI_X_TAKEOFF		0.0
#define KD_X_TAKEOFF		0.08
#define UPPER_X_TAKEOFF		1.0
#define LOWER_X_TAKEOFF		-1.0

#define KP_Y_TAKEOFF		0.4	
#define KI_Y_TAKEOFF		0.0
#define KD_Y_TAKEOFF		0.08
#define UPPER_Y_TAKEOFF		1.0
#define LOWER_Y_TAKEOFF		-1.0

#define KP_Z_TAKEOFF 		0.3
#define KI_Z_TAKEOFF		0.0
#define KD_Z_TAKEOFF		0.35
#define UPPER_Z_TAKEOFF		2.0
#define LOWER_Z_TAKEOFF 	-1.0

//flight controller
#define KP_X_FLIGHT			0.4
#define KI_X_FLIGHT			0.0001
#define KD_X_FLIGHT			0.08
#define UPPER_X_FLIGHT		12.0
#define LOWER_X_FLIGHT		-12.0

#define KP_Y_FLIGHT			0.4
#define KI_Y_FLIGHT			0.0001
#define KD_Y_FLIGHT			0.08
#define UPPER_Y_FLIGHT		12.0
#define LOWER_Y_FLIGHT		-12.0

#define KP_Z_FLIGHT			0.3
#define KI_Z_FLIGHT			0.0
#define KD_Z_FLIGHT			0.35
#define UPPER_Z_FLIGHT		3.5
#define LOWER_Z_FLIGHT		-3.5

//land controller
#define KP_X_LAND			0.4
#define KI_X_LAND			0.0001
#define KD_X_LAND			0.08
#define UPPER_X_LAND		0.5
#define LOWER_X_LAND		-0.5

#define KP_Y_LAND			0.4
#define KI_Y_LAND			0.0001
#define KD_Y_LAND			0.08
#define UPPER_Y_LAND		0.5
#define LOWER_Y_LAND		-0.5

#define KP_Z_LAND			0.5
#define KI_Z_LAND			0.0
#define KD_Z_LAND			0.25
#define UPPER_Z_LAND		1.0
#define LOWER_Z_LAND		-1.0


//yaw controller
#define KP_YAW				0.6
#define KI_YAW				0.0
#define KD_YAW				0.3
#define UPPER_YAW			0.785
#define LOWER_YAW			-0.785 


// -----------------------------------------------------------------------------
//  enumeration & typedef
// -----------------------------------------------------------------------------


//flight state
#ifndef HAVE_ENUM_FLIGHT_STATE
#define HAVE_ENUM_FLIGHT_STATE
typedef enum _FLIGHT_STATE
{
	FLIGHT_STATE_STANDBY = 0,
	FLIGHT_STATE_READY,
	FLIGHT_STATE_TAKEOFF,
	FLIGHT_STATE_SET_INITIAL_YAW,
	FLIGHT_STATE_FLIGHT,
	FLIGHT_STATE_LAND,
	FLIGHT_STATE_EMER,
	FLIGHT_STATE_MISSION_COMPLETE,
	FLIGHT_STATE_MANUAL,
}FLIGHT_STATE;
#endif

//speed set
#ifndef HAVE_STRUCT_SPEED_SET_T
#define HAVE_STRUCT_SPEED_SET_T
typedef struct _speed_set_t
{
	float speed_h;  	//horizontal speed
	float speed_v;		//vertical speed
}speed_set_t;
#endif

// ------------------------------------------------------------------------------
//   Prototypes
// ------------------------------------------------------------------------------

//get array size
template <class T>
int getArrayLen(T& array)

{
	return (sizeof(array) / sizeof(array[0]));
}


int main(int argc, char **argv);

int top(int argc, char **argv);

void fly(autopilot_interface_c &api, xy_network_c &xy_network, xy_vision_c &xy_vision, ultrasonic_c &xy_ultra, waypoint_list_c &waypoints_flight, xy_health_c &xy_health, wireless_c& wireless_serial);

uint16_t init_mission(waypoint_list_c &waypoints, xy_network_c &xy_network, autopilot_interface_c &api);

int flight_takeoff(autopilot_interface_c &api, waypoint_t takeoff_point, ref_navigator_c &navigator_takeoff, controller_3D_posPID_c &controller_takeoff, controller_angPID_c &controller_yaw, xy_vision_c &xy_vision, ultrasonic_c &xy_ultra, integrator_time_c &integrator_takeoff);
uint16_t flight_normal(autopilot_interface_c &api, waypoint_list_c waypoints, ref_navigator_c &ref_cmd, controller_3D_posPID_c &controller_flight, controller_angPID_c &controller_yaw, int item, float acceptance_radius);
int flight_land(autopilot_interface_c &api, waypoint_t land_point, ref_navigator_c &navigator_land, controller_3D_posPID_c &controller_land, controller_angPID_c &controller_yaw, xy_vision_c &xy_vision, ultrasonic_c &xy_ultra, integrator_time_c &integrator_land, xy_network_c &xy_network);

ref_position_vision_t correction_vision(ref_position_vision_t vision_raw, autopilot_interface_c api, float height);

// quit handler
autopilot_interface_c *autopilot_interface_quit;
serial_port_c *serial_port_quit;
wireless_c *wireless_serial_quit;

void quit_handler( int sig );

float dis_cal(mavlink_local_position_ned_t point_1, waypoint_t point_2);
float error_cal(mavlink_set_position_target_local_ned_t point_1, mavlink_local_position_ned_t point_2);
void set_pid_online(controller_3D_posPID_c &controller_1, controller_angPID_c &controller_2, d3_PID_t &PID_t_1, PID_kit_t &PID_t_2, float &radius, xy_network_c &network_commander);
void distance_transformation(float x_in, float y_in, float &x_out, float &y_out, float height, xy_vision_c vision);


#endif

